/*
 * matrix.cc
 *
 * Esercitazione 1 dell'Insegnamento di Robotica.
 * Laurea Specialistica in Ingegneria Informatica,
 * Universita' degli Studi di Parma
 * Responsabile: Dario Lodi Rizzini
 */
#include "matrix.h"
#include <cmath>
#include <cstdlib>

using namespace std;

// ----------------------------------------------------------------------
//  SIMPLE VECTOR TYPE
// ----------------------------------------------------------------------

Vector::Vector(int size) : values_(new double[size]), size_(size) 
{ 
  for (int i = 0; i < size_; ++i)
    values_[i] = 0.0;
}
  
Vector::~Vector() 
{
  delete [] values_;
}
  
int Vector::size() const
{
  return size_;
}
   
double Vector::value(int i) const
{
  assert(0 <= i && i < size_);
  return values_[i];
}
  
double& Vector::value(int i)
{
  assert(0 <= i && i < size_);
  return values_[i];
}
  
double& Vector::operator()(int i)
{
  return value(i);
}
  
void Vector::assign(double v)
{
  for (int i = 0; i < size_; ++i)
    values_[i] = v;
}
  
void Vector::assign(const Vector& v)
{
  for (int i = 0; i < size_; ++i)
    values_[i] = v.values_[i];
}
  
void Vector::sum(const Vector& v) const
{
  for (int i = 0; i < size_; ++i) {
    values_[i] = v.values_[i] * values_[i];
  }
}
  
double Vector::prod(const Vector& v) const
{
  double p = 0.0;
//   cout << "Start product: " << endl;
  for (int i = 0; i < size_; ++i) {
    p += values_[i] * v.values_[i];
//     cout << " + " << values_[i] << " * " << v.values_[i] << "(p = " << p << ")"<< endl;
  }
//   cout << "Result: " << p << endl;
  return p;
}
  
void Vector::prod(double s)
{
  for (int i = 0; i < size_; ++i) {
    values_[i] = s * values_[i];
  }
}
  
double Vector::norm2() const
{
  double v = prod(*this);
  assert(v >= 0.0);
  v = ::sqrt(v);
  return v;
}
  
void Vector::normalize()
{
  double v = norm2();
  for (int i = 0; i < size_; ++i) {
    values_[i] = values_[i] / v;
  }
}
  
void Vector::swap(Vector& v)
{
  assert(v.size_ == size_);
  double* tmp = v.values_;
  v.values_ = values_;
  values_ = tmp;
}


// ----------------------------------------------------------------------
//  SIMPLE SYMMETRIC MATRIX TYPE
// ----------------------------------------------------------------------

SymmetricMatrix::SymmetricMatrix(int size) 
 : values_(new double[size*(size+1)/2]), size_(size) 
{ 
  int s = (size_*(size_+1)/2);
  for (int i = 0; i < s; ++i) 
    values_[i] = 0.0;
}
  
SymmetricMatrix::~SymmetricMatrix() 
{
  delete [] values_;
}
  
int SymmetricMatrix::size() const
{
  return size_;
}
  
double SymmetricMatrix::value(int i,int j) const
{
  int tmp;
  if (i < 0 || i >= size_ || j < 0 || j >= size_) {
    std::cerr << __FILE__ << "," << __LINE__ << ": indexes " << i << "," << j << " out of bound " << size_ << std::endl;
  }
  assert(0 <= i && i < size_);
  assert(0 <= j && j < size_);
  if (i < j) {
    tmp = i;
    i = j;
    j = tmp;
  }
  return values_[i*(i+1)/2+j];
}
  
double& SymmetricMatrix::value(int i,int j)
{
  int tmp;
  if (i < 0 || i >= size_ || j < 0 || j >= size_) {
    std::cerr << __FILE__ << "," << __LINE__ << ": indexes " << i << "," << j << " out of bound " << size_ << std::endl;
  }
  assert(0 <= i && i < size_);
  assert(0 <= j && j < size_);
  if (i < j) {
    tmp = i;
    i = j;
    j = tmp;
  }
  return values_[i*(i+1)/2+j];
}
  
double& SymmetricMatrix::operator()(int i,int j)
{
  return value(i,j);
}
  
double SymmetricMatrix::getMax() const
{
  double m = std::numeric_limits<double>::min();
  int s = (size_*(size_+1)/2);
  for (int i = 0; i < s; ++i) {
    if (values_[i] > m)
      m = values_[i];
  }
  return m;
}
  
double SymmetricMatrix::getMin() const
{
  double m = std::numeric_limits<double>::max();
  int s = (size_*(size_+1)/2);
  for (int i = 0; i < s; ++i) {
    if (values_[i] < m)
      m = values_[i];
  }
  return m;
}
  
void SymmetricMatrix::prod(Vector& r,const Vector& v)
{
  assert(r.size() == size_ && v.size() == size_);
  for (int i = 0; i < size_; ++i) {
    r(i) = 0.0;
    for (int j = 0; j < size_; ++j) {
      r(i) += value(i,j) * v.value(i);
    }
  }
}
  
// It uses Lanczos algorithm to compute main eigenvector
void SymmetricMatrix::computeMainEigenVectorLanczos(Vector& v,int nstep)
{
  // It checks the size of resulting vector
  if (v.size() < size_) {
    std::cerr << __FILE__ << "," << __LINE__ << ": invalid size of vector: " 
             << v.size() << " instead of the required " << size_ << std::endl;
  }
  else if (v.size() > size_) {
    std::cerr << __FILE__ << "," << __LINE__ << ": vector is too big: " 
             << v.size() << " instead of the required " << size_ << std::endl;
  }
  assert(v.size() <= size_);
  
  // Vector v is initialized as a random vector with norm 1.0
  std::cerr << __FILE__ << "," << __LINE__ << ": initialize eigenvector randomly" 
            << std::endl;
  for (int i = 0; i < size_; ++i) {
    v(i) = 1.0 * rand() / RAND_MAX;
  }
  std::cout << __FILE__ << "," << __LINE__ << ": eigenvector normalization" 
            << std::endl;
  v.normalize();
  
  Vector w(size_);      // vector of zeros
  Vector tmp(size_);    // vector of zeros
  double alpha = 0.0;   
  double beta = 0.0;
  for (int step = 0; step < nstep; ++step) {
    // Computes w = A * v - beta * w
    for (int i = 0; i < w.size(); ++i) {
      w(i) = 0.0;
      for (int j = 0; j < size_; ++j) 
        w(i) += value(i,j) * v(j);
      w(i) += -beta * w(i);
    }
    // Computes alpha = w * v
    alpha = 0.0;
    for (int i = 0; i < w.size(); ++i) 
      alpha += v(i) * w(i);
    // Computes w = w - alpha * v
    for (int i = 0; i < w.size(); ++i) 
      w(i) = w(i) - alpha * v(i);
    // Computes new value of beta: beta = norm2(w)
    beta = 0.0;
    for (int i = 0; i < w.size(); ++i) 
      beta += w(i) * w(i);
    beta = sqrt(beta);
    // Normalizes w
    for (int i = 0; i < w.size(); ++i) 
      w(i) += w(i) / beta;
    cout << "Approximate eigenvector: " << endl << "[";
    for (int i = 0; i < w.size(); ++i) 
      cout << w(i) << " ";
    cout << "]" << endl;
    v.swap(w);
  }
  cerr << "Approximate eigenvector: " << endl << "[";
  for (int i = 0; i < w.size(); ++i) 
    cout << w(i) << " ";
  cout << "]" << endl;
}

// It uses power method
double SymmetricMatrix::computeMainEigenVectorPower(Vector& v,int nstep)
{
  // It checks the size of resulting vector
  if (v.size() < size_) {
    std::cerr << __FILE__ << "," << __LINE__ << ": invalid size of vector: " 
             << v.size() << " instead of the required " << size_ << std::endl;
  }
  else if (v.size() > size_) {
    std::cerr << __FILE__ << "," << __LINE__ << ": vector is too big: " 
             << v.size() << " instead of the required " << size_ << std::endl;
  }
  assert(v.size() <= size_);
  
  // Vector v is initialized as a random vector with norm 1.0
  std::cerr << __FILE__ << "," << __LINE__ << ": initialize eigenvector" 
            << std::endl;
  for (int i = 0; i < size_; ++i) {
    v(i) = 1.0;
  }
  
  Vector w(size_);      // vector of zeros
  double mu;  
  double lambda; 
  for (int step = 0; step < nstep; ++step) {
    // Computes w = A * v
    for (int i = 0; i < w.size(); ++i) {
      w(i) = 0.0;
      for (int j = 0; j < size_; ++j) 
        w(i) += value(i,j) * v(j);
    }
    // Computes alpha = w * v
    mu = 0.0;
    lambda = 0.0;
    for (int i = 0; i < w.size(); ++i) {
      mu += v(i) * w(i);
      lambda += v(i) * v(i);
    }
    if (fabs(lambda) > 0.0) {
      lambda = mu / lambda;
    }
    mu = sqrt(mu);
    for (int i = 0; i < w.size(); ++i) 
      w(i) = w(i) / mu;
    v.swap(w);
  }
  return lambda; 
}
  
  
void SymmetricMatrix::savePgm(const char* filename) const
{
  std::ofstream out(filename);
  if (!out) {
    std::cerr << "Cannot save file \"" << filename << "\"" << std::endl;
    return;
  }
  out << "P5" << std::endl << size_ << " " << size_ << std::endl 
      << "255" << std::endl;
  double vmax = getMax();
  double vmin = getMin();
  std::cerr << __FILE__ << "," << __LINE__ << ": matrix min " << vmin << ", max " << vmax << std::endl; 
  for (int i = 0; i < size_; ++i)
    for (int j = 0; j < size_; ++j){
      //std::cerr << "  covar(" << i << "," << j << "): " << value(i,j) << std::endl;
      unsigned char c = static_cast<unsigned char>(255.0*log(1.0 + M_E * (value(i,j)-vmin)/(vmax-vmin)));
      //std::cerr << "  color(" << i << "," << j << "): " << (int)c << " from " << 255.0*(value(i,j)-vmin)/(vmax-vmin) << std::endl;
      out << c;
    }
  out.close();
}


